#include "ImageCoordinateGeometry.h"

#include <QString>
#include <QVector>
#include <QMessageBox>

#include "vtkMath.h"
#include "vtkVector.h"
#include "vtkMatrix3x3.h"

ImageCoordinateGeometry::ImageCoordinateGeometry(ImageProperties &imageProperties)
{
    m_IsOblique = false;

    m_DirectionMatrix = vtkMatrix3x3::New();
    this->ConvertOrientationToVTKMatrix(imageProperties.imageOrientationVectors[0],
                                        imageProperties.imageOrientationVectors[1]);
    this->IsDirectionMatrixOblique();
    m_ImagePosition = imageProperties.imagePosition;

    this->SetDefaultTransformMatrices();
    this->CalculateTransformMatrices();
}

void
ImageCoordinateGeometry::SetDefaultTransformMatrices()
{
    m_AxialTransformMatrix = vtkMatrix4x4::New();
    m_SaggitalTransformMatrix = vtkMatrix4x4::New();
    m_CoronalTransformMatrix = vtkMatrix4x4::New();

    double axialArray[16] = {1, 0, 0, 0,
                             0, 1, 0, 0,
                             0, 0, 1, 0,
                             0, 0, 0, 1};
    m_AxialTransformMatrix->DeepCopy(axialArray);

    double saggitalArray[16] = {0, 0,-1, 0,
                                1, 0, 0, 0,
                                0,-1, 0, 0,
                                0, 0, 0, 1};
    m_SaggitalTransformMatrix->DeepCopy(saggitalArray);

    double coronalArray[16] = {1, 0, 0, 0,
                               0, 0, 1, 0,
                               0,-1, 0, 0,
                               0, 0, 0, 1};
    m_CoronalTransformMatrix->DeepCopy(coronalArray);
}

void
ImageCoordinateGeometry::ConvertOrientationToVTKMatrix(QVector<double> orientationX,
                                                       QVector<double> orientationY)
{
    double X[3], Y[3], Z[3], elements[9];
    DirectionMatrixPointer directionMatrix = vtkMatrix3x3::New();
    for (int i = 0; i < 3; i++)
    {
        X[i] = orientationX[i];
        Y[i] = orientationY[i];
    }
    vtkMath::Cross(X,Y,Z);
    for (int j = 0; j < 3; j++)
    {
        elements[j] = X[j];
        elements[j+3] = Y[j];
        elements[j+6] = Z[j];
    }

    m_DirectionMatrix->DeepCopy(elements);
    m_LPSCode = DetermineClosestLPSCode(directionMatrix);
}

void
ImageCoordinateGeometry::CalculateTransformMatrices()
{
    m_InvertedDirectionMatrix = vtkMatrix3x3::New();
    vtkMatrix3x3::Invert(m_DirectionMatrix, m_InvertedDirectionMatrix);

    m_AxialTransformMatrix = vtkMatrix4x4::New();
    m_AxialTransformMatrix->Zero();
    m_AxialTransformMatrix->SetElement(3,3,1.0);
    for (int i = 0; i < 3; i++)
        for (int j = 0; j <= 3; j++)
            m_AxialTransformMatrix->SetElement(i,j,m_InvertedDirectionMatrix->GetElement(i,j));

    m_AxialTransformMatrix->SetElement(0, 3, -m_ImagePosition[0]);
    m_AxialTransformMatrix->SetElement(1, 3, -m_ImagePosition[1]);
    m_AxialTransformMatrix->SetElement(2, 3, -m_ImagePosition[2]);

}

QString
ImageCoordinateGeometry::DetermineClosestLPSCode(DirectionMatrixPointer directionMatrix)
{
    const static QString LPS_start("LPS"), LPS_end("RAI");
    QString LPSCode;
    vtkVector<double,3> column;

    for(int j = 0; j < 3; j++)
    {
        double maxElement = 0;
        int axis = 0;
        for (int i = 0; i < 3; i++)
        {
            column[i] = directionMatrix->GetElement(i,j);
            if (abs(column[i]) > maxElement)
            {
                maxElement = column[i];
                axis = i;
            }
        }
        LPSCode[j] = maxElement > 0 ? LPS_start[j] : LPS_end[j];
    }
    return LPSCode;
}

bool
ImageCoordinateGeometry::IsDirectionMatrixOblique()
{
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            if ((fabs(m_DirectionMatrix->GetElement(i,j)) > 0) && (fabs(m_DirectionMatrix->GetElement(i,j)) < 1))
                m_IsOblique = true;
        }
    }
    return m_IsOblique;
}

double*
ImageCoordinateGeometry::GetITKWorldPoint(double *VTKpoint, double *ITKPoint)
{
    ITKPoint[0] = VTKpoint[0] + m_ImagePosition[0];
    ITKPoint[1] = VTKpoint[1] + m_ImagePosition[1];
    ITKPoint[2] = VTKpoint[2] + m_ImagePosition[2];

    return ITKPoint;
}
